#include "bsp_can.h"
#include "main.h"
#include "bsp_can_recieve.h"
#include "CAN_receive.h"
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;


//电机数据整合
#define get_motor_measure_measure(ptr, data)                    								\
    {                                                                 	\
        (ptr)->last_ecd = (ptr)->ecd;                              			\
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6]; 																	\
				(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt);																	\
				(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt);																\
				(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE;														\
    }//ptr指向电机测量数据结构体的指针，data包含电机测量数据的数组.

#define get_motor_offset_measure(ptr, data)														\
		{																																		\
				(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
				(ptr)->offset_ecd = (ptr)->ecd;																	\
		}

static motor_measure_t      motor_measure[7];
static CAN_TxHeaderTypeDef  tx_message;
static uint8_t              can_send_data[8];



static uint16_t rx_header_ID;//用来检测电机的发送报文标识符


void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	
	BSP_CAN_t bsp_can = CAN_Get(hcan);
     if (bsp_can!=BSP_CAN_ERR) {
      if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB])
      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB]();
     }//错误处理
    uint8_t rx_data[8];

    CAN_RxHeaderTypeDef rx_header;
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
	
    switch (rx_header.StdId)
    {
        case 0x201:
        case 0x202:
        case 0x203:
        case 0x204:
        case 0x205:
        case 0x206:
        case 0x207:
        {
            static uint8_t i = 0;
            i = rx_header.StdId - 0x201;
					if(motor_measure[i].msg_cnt<=50)
					{
						motor_measure[i].msg_cnt++;
						get_motor_offset_measure(&motor_measure[i], rx_data);
					}else{
						get_motor_measure_measure(&motor_measure[i], rx_data);
					}
            break;
        }
	   		
        default:
        {
            break;
        }
    }
    
}




void CAN_Motor_SendData(CAN_HandleTypeDef * hcan,uint16_t Send_Message_ID,uint16_t Motor_ID,int16_t Motor_data)//单电机控制
{
    uint32_t send_mail_box;
    rx_header_ID=Send_Message_ID;
	uint16_t id;
	
	switch(rx_header_ID)
	{		
		case 0x200:
               id= Motor_ID-0x200;break ;
		case 0x1ff:
               id= Motor_ID-0x204;break ;
		case 0x2ff:
			id= Motor_ID-0x208;break ;
			
	}
    tx_message.StdId = Send_Message_ID;
    tx_message.IDE = CAN_ID_STD;
    tx_message.RTR = CAN_RTR_DATA;
    tx_message.DLC = 0x08;
    can_send_data[id*2-2] = (Motor_data >> 8);
    can_send_data[id*2-1] = Motor_data;

    HAL_CAN_AddTxMessage(hcan, &tx_message,can_send_data, &send_mail_box);

}



void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)//四电机控制，适用于0x201-204
{
    uint32_t send_mail_box; //存储发送消息的邮箱标识符
    tx_message.StdId = 0x200;  //送给所有底盘电机的CAN标识符
    tx_message.IDE = CAN_ID_STD;   //表示使用标准的CAN标识符
    tx_message.RTR = CAN_RTR_DATA;  //数据帧
    tx_message.DLC = 0x08;     //8个字节
    can_send_data[0] = motor1 >> 8;														
    can_send_data[1] = motor1;
    can_send_data[2] = motor2 >> 8;
    can_send_data[3] = motor2;
    can_send_data[4] = motor3 >> 8;
    can_send_data[5] = motor3;
    can_send_data[6] = motor4 >> 8;
    can_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
}



static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);

static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
  if (hcan->Instance == CAN2)
    return BSP_CAN_2;
  else if (hcan->Instance == CAN1)
    return BSP_CAN_1;
  else
    return BSP_CAN_ERR;
}



void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {
  BSP_CAN_t bsp_can = CAN_Get(hcan);
  if (bsp_can != BSP_CAN_ERR) {
    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB])
      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB]();
  }
}


CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
  switch (can) {
    case BSP_CAN_2:
      return &hcan2;
    case BSP_CAN_1:
      return &hcan1;
    default:
      return NULL;
  }
}


int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
                                void (*callback)(void)) {
  if (callback == NULL) return BSP_ERR_NULL;
  CAN_Callback[can][type] = callback;
  return BSP_OK;
}


const motor_measure_t *get_motor_measure_point(uint8_t i)
{
    return &motor_measure[(i & 0x07)];    //确保i的值在0到7之间
}


